/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-08     Hao       the first version
 */
#ifndef SIMPLEFOC_BLDC_MOTOR_H_
#define SIMPLEFOC_BLDC_MOTOR_H_

#include "encoder.h"
#include "driver.h"
#include "inline_current_sense.h"
#include "foc_utils.h"
#include "time_utils.h"
#include "pid.h"
#include "lowpass_filter.h"
#include "default.h"

// 运动控制模式
enum MotionControlType
{
    TORQUE = 0,             // Torque control
    VELOCITY = 1,           // Velocity motion control
    ANGLE = 2,              // Position/angle motion control
    VELOCITY_OPENLOOP = 3,
    ANGLE_OPENLOOP = 4
};

// 电流环控制模式
enum TorqueControlType
{
    VOLTAGE = 0x00,     // Torque control using voltage
    DC_CURRENT = 0x01,  // Torque control using DC current (one current magnitude)
    FOC_CURRENT = 0x02, // torque control using dq currents
};

// 电机状态
enum FOCMotorStatus
{
    motor_uninitialized = 0x00, // Motor is not yet initialized
    motor_initializing = 0x01,  // Motor intiialization is in progress
    motor_uncalibrated = 0x02,  // Motor is initialized, but not calibrated (open loop possible)
    motor_calibrating = 0x03,   // Motor calibration in progress
    motor_ready = 0x04,         // Motor is initialized and calibrated (closed loop possible)
    motor_error = 0x08,         // Motor is in error state (recoverable, e.g. overcurrent protection active)
    motor_calib_failed = 0x0E,  // Motor calibration failed (possibly recoverable)
    motor_init_failed = 0x0F,   // Motor initialization failed (not recoverable)
};

/**
 BLDC motor class
 */
class BLDCMotor
{
public:
    BLDCMotor(int pp);  // 极对数
    ~BLDCMotor();       // 极对数
    int pole_pairs_;    // motor 极对数

    // 基操
    void init();

    // 电机状态定义
    int8_t enabled_; // enabled or disabled motor flag
    void enable();
    void disable();

    // 磁传感器
    Encoder *encoder_;                  // 磁传感器
    float zero_electric_angle_;         // 电机0度时，传感器角度对应的电角度
    Directions encoder_direction_;      // 传感器方向
    void linkEncoder(Encoder *encoder); // 绑定磁传感器
    float voltage_encoder_align_;       // 传感器和电机对齐电压参数
    int alignEncoder();

    // 电流传感器
    InlineCurrentSense *current_sense_;                         // 电流传感器
    void linkCurrentSense(InlineCurrentSense *current_sense);   // 绑定电流传感器

    // 电机驱动器
    Driver *driver_;                    // 电机驱动器
    void linkDriver(Driver *driver);    // 绑定电机驱动器

    // 控制模式
    TorqueControlType torque_controller_;   // 扭矩控制模式
    MotionControlType motion_controller_;   // 运动控制模式

    // 控制目标，target和用户有关，其他的都是过程中的参数
    float target_;            // 当前目标值，和控制模式有关，目标位置、速度、扭矩，app就控制这个变量
    float current_sp_;        // 当前目标电流q（控制过程变量）.只使用 q 控制时使用
    float shaft_velocity_sp_; // 当前目标速度（控制过程变量）
    float shaft_angle_sp_;    // 当前目标角度（控制过程变量）
    DQVoltage_s voltage_;     // 当前设置到电机的 d 和 q 电压
    DQCurrent_s current_;     // 当前测量的电流 d 和 q

    // PID、LPF参数
    // 电流环
    PIDController PID_current_q_ { DEF_PID_CURR_P, DEF_PID_CURR_I, DEF_PID_CURR_D, DEF_PID_CURR_RAMP, DEF_PID_CURR_LIMIT };
    PIDController PID_current_d_ { DEF_PID_CURR_P, DEF_PID_CURR_I, DEF_PID_CURR_D, DEF_PID_CURR_RAMP, DEF_PID_CURR_LIMIT };
    LowPassFilter LPF_current_q_ { DEF_CURR_FILTER_Tf };
    LowPassFilter LPF_current_d_ { DEF_CURR_FILTER_Tf };
    // 速度环
    PIDController PID_velocity_ { DEF_PID_VEL_P, DEF_PID_VEL_I, DEF_PID_VEL_D, DEF_PID_VEL_RAMP, DEF_PID_VEL_LIMIT };
    LowPassFilter LPF_velocity_ { DEF_VEL_FILTER_Tf };
    // 角度环
    PIDController P_angle_ { DEF_P_ANGLE_P, 0, 0, 0, DEF_VEL_LIM };
    LowPassFilter LPF_angle_ { 0.0 };

    // 控制过程限制参数
    float voltage_limit_;  // Voltage limiting variable - global limit
    float current_limit_;  // Current limiting variable - global limit
    float velocity_limit_; // Velocity limiting variable - global limit

    // 角度计算相关
    float shaft_angle_;         // 当前电机轴角度
    float electrical_angle_;    // 当前电机电角度
    float shaft_velocity_;      // 当前电机速度
    float shaftAngle();         // 获取轴角度[rad]
    float shaftVelocity();      // 获取轴速度[rad/s]
    float electricalAngle();    // 计算电角度

    // 用户控制接口**
    int initFOC();                     // 初始化foc，对准传感器和电机的0位
    void move(float target = NOT_SET); // 计算力矩环目标值
    void loopFOC();                    // 运行力矩换控制，越快越好
    void setPhaseVoltage(float Uq, float Ud, float angle_el);

    // 开环控制
    long open_loop_timestamp_;
    float velocityOpenloop(float target_velocity);
    float angleOpenloop(float target_angle);
};

#endif /* SIMPLEFOC_BLDC_MOTOR_H_ */
